﻿using System;

namespace FortuneUniverse.Fortune.Algorithms
{
  public struct Pose : IEquatable<Pose>
  {
    public Vector3 position;
    public Quaternion rotation;
    private static readonly Pose k_Identity = new Pose(Vector3.zero, Quaternion.identity);

    public Pose(Vector3 position, Quaternion rotation)
    {
      this.position = position;
      this.rotation = rotation;
    }

    public Pose GetTransformedBy(Pose lhs) => new Pose()
    {
      position = lhs.position + lhs.rotation * this.position,
      rotation = lhs.rotation * this.rotation
    };

    public Vector3 forward => this.rotation * Vector3.forward;

    public Vector3 right => this.rotation * Vector3.right;

    public Vector3 up => this.rotation * Vector3.up;

    public static Pose identity => Pose.k_Identity;

    public override bool Equals(object obj) => obj is Pose other && this.Equals(other);

    public bool Equals(Pose other) => this.position == other.position && this.rotation == other.rotation;

    public override int GetHashCode() => this.position.GetHashCode() ^ this.rotation.GetHashCode() << 1;

    public static bool operator ==(Pose a, Pose b) => a.Equals(b);

    public static bool operator !=(Pose a, Pose b) => !(a == b);
  }
}
